use std::{
    collections::BTreeMap,
    net::SocketAddr,
    sync::{Arc, Mutex},
};

use log::error;
use tokio::sync::mpsc;

use crate::{Error, Result};

/// 连接到carla,要求先启动carla进程.
pub async fn connect(addr: SocketAddr, timeout_ms: u32) -> Result<(Proxy, RuntimeProxy)> {
    let ctx = Arc::new(Mutex::new(Context {
        vehicles: BTreeMap::new(),
    }));
    let proxy = create_proxy(addr, timeout_ms, ctx.clone())
        .await
        .map(|inner| Proxy { inner })?;
    let runtime_proxy = create_proxy(addr, timeout_ms, ctx)
        .await
        .map(|inner| RuntimeProxy { inner })?;

    Ok((proxy, runtime_proxy))
}

async fn create_proxy(addr: SocketAddr, timeout_ms: u32, ctx: ContextMt) -> Result<Inner> {
    let (req_tx, mut req_rx) = mpsc::channel(1);
    let (res_tx, mut res_rx) = mpsc::channel(1);

    // carla-rust提供的是同步调用接口,这里使用线程实现异步接口
    std::thread::spawn(move || {
        // 创建客户端
        let mut client =
            carla::client::Client::new(addr.ip().to_string().as_str(), addr.port(), 0u32);
        client.set_timeout(timeout_ms);

        // 获取世界
        match client.get_world() {
            Ok(mut world) => {
                // 删除车辆相关传感器函数
                let delete_vehicle_sensors =
                    |world: &mut carla::client::World, vehicle: &mut Vehicle| {
                        if let Some(base_sensors) = vehicle.base_sensors.take() {
                            world.destroy_actor_by_id(base_sensors.gnss.get_id()).ok();
                            world.destroy_actor_by_id(base_sensors.cd.get_id()).ok();
                            world.destroy_actor_by_id(base_sensors.lid.get_id()).ok();
                        }

                        vehicle.cameras.values().for_each(|camera| {
                            world.destroy_actor_by_id(camera.get_id()).ok();
                        });

                        vehicle.cameras.clear();
                    };

                // 发送准备信号
                if res_tx.blocking_send(Response::Ready(Ok(()))).is_ok() {
                    // 信号接收-响应循环
                    while let Some(msg) = req_rx.blocking_recv() {
                        match msg {
                            // 地图载入请求
                            Request::LoadMap { content } => {
                                let r = client
                                    .generate_open_drive_world(
                                        content.as_str(),
                                        2.0,
                                        1000.0,
                                        1.0,
                                        0.5,
                                        false,
                                        true,
                                        true,
                                        true,
                                    )
                                    .map(|w| {
                                        world = w;
                                    })
                                    .map_err(Error::from);

                                if res_tx.blocking_send(Response::LoadMap(r)).is_err() {
                                    break;
                                }
                            }

                            // 创建车辆请求
                            Request::SpawnVehicle {
                                vehicle_type,
                                location_xyz,
                                rotation_pyr,
                            } => {
                                let r = world
                                    .spawn_vehicle(
                                        vehicle_type.as_str(),
                                        location_xyz,
                                        rotation_pyr,
                                    )
                                    .map_err(Error::from)
                                    .map(|vehicle| {
                                        let vehicle_id = vehicle.get_id();

                                        ctx.lock().unwrap().vehicles.insert(
                                            vehicle_id,
                                            Vehicle {
                                                vehicle,
                                                base_sensors: None,
                                                cameras: BTreeMap::new(),
                                            },
                                        );

                                        vehicle_id.to_string()
                                    });

                                if res_tx.blocking_send(Response::SpawnVehicle(r)).is_err() {
                                    break;
                                }
                            }

                            // 根据角色名获取已存在车辆请求
                            Request::GetVehicleByRole { role_name } => {
                                let r = world
                                    .get_vehicle_by_role(role_name.as_str())
                                    .map_err(Error::from)
                                    .and_then(|vehicle| {
                                        if let Some(vehicle) = vehicle {
                                            let vehicle_id = vehicle.get_id();
                                            let vehicle_type_id = vehicle.get_type_id();

                                            ctx.lock().unwrap().vehicles.insert(
                                                vehicle_id,
                                                Vehicle {
                                                    vehicle,
                                                    base_sensors: None,
                                                    cameras: BTreeMap::new(),
                                                },
                                            );

                                            Ok((vehicle_id.to_string(), vehicle_type_id))
                                        } else {
                                            Err(Error::new("Can not find vehicle".to_string()))
                                        }
                                    });

                                if res_tx.blocking_send(Response::GetVehicleByRole(r)).is_err() {
                                    break;
                                }
                            }

                            // 启用车辆的carsim仿真请求
                            Request::EnableCarsim {
                                vehicle_id,
                                carsim_cfg_path,
                            } => {
                                let r = vehicle_id.parse().map_err(Error::from).and_then(|id| {
                                    let vehicle = ctx.lock().unwrap().vehicles.remove(&id);

                                    vehicle.map_or(
                                        Err(Error::new("Can not find vehicle".to_string())),
                                        |mut vehicle| {
                                            let r = vehicle
                                                .vehicle
                                                .enable_carsim(carsim_cfg_path.as_str())
                                                .map_err(Error::from);

                                            ctx.lock().unwrap().vehicles.insert(id, vehicle);

                                            r
                                        },
                                    )
                                });

                                if res_tx.blocking_send(Response::EnableCarsim(r)).is_err() {
                                    break;
                                }
                            }

                            // 启用车辆基本传感器请求
                            Request::EnableBaseSensors {
                                vehicle_id,
                                location_xyz,
                                rotation_pyr,
                                sensor_tick,
                            } => {
                                let r = vehicle_id.parse().map_err(Error::from).and_then(|id| {
                                    let vehicle = ctx.lock().unwrap().vehicles.remove(&id);

                                    vehicle.map_or(
                                        Err(Error::new("Can not find vehicle".to_string())),
                                        |mut vehicle| {
                                            let r = world
                                                .create_gnss(
                                                    id,
                                                    location_xyz,
                                                    rotation_pyr,
                                                    sensor_tick,
                                                )
                                                .and_then(|gnss| {
                                                    world
                                                        .create_collision_detector(
                                                            id,
                                                            location_xyz,
                                                            rotation_pyr,
                                                        )
                                                        .and_then(|cd| {
                                                            world
                                                                .create_lane_invasion_detector(
                                                                    id,
                                                                    location_xyz,
                                                                    rotation_pyr,
                                                                )
                                                                .map(|lid| {
                                                                    vehicle.base_sensors =
                                                                        Some(BaseSensors {
                                                                            gnss,
                                                                            twist: Twist::new(),
                                                                            cd,
                                                                            lid,
                                                                        });
                                                                })
                                                        })
                                                })
                                                .map_err(Error::from);

                                            ctx.lock().unwrap().vehicles.insert(id, vehicle);

                                            r
                                        },
                                    )
                                });

                                if res_tx
                                    .blocking_send(Response::EnableBaseSensors(r))
                                    .is_err()
                                {
                                    break;
                                }
                            }

                            // 启用摄像机请求
                            Request::EnableCamera {
                                vehicle_id,
                                camera_id,
                                location_xyz,
                                rotation_pyr,
                                fov,
                                image_size_x,
                                image_size_y,
                                iso,
                                gamma,
                                shutter_speed,
                                sensor_tick,
                            } => {
                                let r = vehicle_id.parse().map_err(Error::from).and_then(|id| {
                                    let vehicle = ctx.lock().unwrap().vehicles.remove(&id);

                                    vehicle.map_or(
                                        Err(Error::new("Can not find vehicle".to_string())),
                                        |mut vehicle| {
                                            let r = world
                                                .create_camera(
                                                    id,
                                                    location_xyz,
                                                    rotation_pyr,
                                                    fov,
                                                    image_size_x,
                                                    image_size_y,
                                                    iso,
                                                    gamma,
                                                    shutter_speed,
                                                    sensor_tick,
                                                )
                                                .map(|camera| {
                                                    vehicle.cameras.insert(camera_id, camera);
                                                })
                                                .map_err(Error::from);

                                            ctx.lock().unwrap().vehicles.insert(id, vehicle);

                                            r
                                        },
                                    )
                                });

                                if res_tx.blocking_send(Response::EnableCamera(r)).is_err() {
                                    break;
                                }
                            }

                            // 设置车辆位置
                            Request::SetTransform {
                                vehicle_id,
                                location_xyz,
                                rotation_pyr,
                            } => {
                                let r = vehicle_id.parse().map_err(Error::from).and_then(|id| {
                                    let vehicle = ctx.lock().unwrap().vehicles.remove(&id);

                                    vehicle.map_or(
                                        Err(Error::new("Can not find vehicle".to_string())),
                                        |mut vehicle| {
                                            let r = vehicle
                                                .vehicle
                                                .set_transform(location_xyz, rotation_pyr)
                                                .map_err(Error::from);

                                            ctx.lock().unwrap().vehicles.insert(id, vehicle);

                                            r
                                        },
                                    )
                                });

                                if res_tx.blocking_send(Response::SetTransform(r)).is_err() {
                                    break;
                                }
                            }

                            // 设置车辆控制参数请求
                            Request::ApplyVehicleControl {
                                vehicle_id,
                                control,
                            } => {
                                vehicle_id
                                    .parse()
                                    .map(|id| {
                                        if let Some(vehicle) =
                                            ctx.lock().unwrap().vehicles.get_mut(&id)
                                        {
                                            vehicle.vehicle.apply_control(&control);
                                        }
                                    })
                                    .ok();
                            }

                            // 删除车辆请求
                            Request::DestroyVehicle {
                                vehicle_id,
                                created,
                            } => {
                                let r = vehicle_id
                                    .parse()
                                    .map(|id| {
                                        let vehicle = ctx.lock().unwrap().vehicles.remove(&id);

                                        if let Some(mut vehicle) = vehicle {
                                            delete_vehicle_sensors(&mut world, &mut vehicle);
                                        }

                                        if created {
                                            // TODO: 忽略错误
                                            world.destroy_actor_by_id(id).ok();
                                        }
                                    })
                                    .map_err(Error::from);

                                if res_tx.blocking_send(Response::DestroyVehicle(r)).is_err() {
                                    break;
                                }
                            }

                            // 删除所有车辆请求
                            Request::DestroyAllVehicles => {
                                let ids = {
                                    let mut ctx = ctx.lock().unwrap();

                                    let ids = ctx.vehicles.keys().cloned().collect::<Vec<_>>();

                                    ctx.vehicles.clear();

                                    ids
                                };

                                ids.iter().for_each(|id| {
                                    world.destroy_actor_by_id(*id).ok();
                                });

                                if res_tx.blocking_send(Response::DestroyAllVehicles).is_err() {
                                    break;
                                }
                            }

                            // 获取当前地图名称请求
                            Request::GetMapName => {
                                let r = world
                                    .get_map()
                                    .map(|map| map.get_name())
                                    .map_err(Error::from);

                                if res_tx.blocking_send(Response::GetMapName(r)).is_err() {
                                    break;
                                }
                            }

                            // 获取世界状态请求
                            Request::GetSnapshot => {
                                let r = world
                                    .get_snapshot()
                                    .map_err(|_| Error::new("Logic error".to_string()))
                                    .map(|snapshot| {
                                        let running_time = snapshot.get_running_time();

                                        let vehicle_sensors = ctx
                                            .lock()
                                            .unwrap()
                                            .vehicles
                                            .iter_mut()
                                            .filter_map(|(id, vehicle)| {
                                                let cameras = &mut vehicle.cameras;
                                                let vehicle_snapshot =
                                                    snapshot.find_actor_snapshot(*id);
                                                vehicle.base_sensors.as_mut().map(
                                                    |base_sensors| {
                                                        (id, vehicle_snapshot, cameras, base_sensors)
                                                    },
                                                )
                                            })
                                            .map(|(id, vehicle_snapshot, cameras, base_sensors)| {
                                                let localization = base_sensors
                                                    .gnss
                                                    .get_localization()
                                                    .and_then(|loc| {
                                                        if let Some(vs) = &vehicle_snapshot {
                                                            // TODO: 时间上不精确
                                                            // TODO: 假设传感器位于车辆中心
                                                            let velocity = vs.get_velocity();
                                                            let angular_velocity = vs.get_angular_velocity();
                                                            let acceleration = vs.get_acceleration();

                                                            Some(Localization {
                                                                base: loc,
                                                                velocity,
                                                                angular_velocity,
                                                                acceleration,
                                                            })
                                                        } else {
                                                            let timestamp = loc.get_timestamp();
                                                            let location = loc.get_location();
                                                            let rotation = loc.get_rotation();

                                                            base_sensors
                                                                .twist
                                                                .update(
                                                                    timestamp, location,
                                                                    rotation,
                                                                )
                                                                .map(|(velocity, angular_velocity, acceleration)| {
                                                                    Localization {
                                                                        base: loc,
                                                                        velocity,
                                                                        angular_velocity,
                                                                        acceleration,
                                                                    }
                                                                })
                                                        }
                                                    });

                                                let ct = vehicle_snapshot.map(|vs| {
                                                    let chassis = Some(Chassis {
                                                        timestamp: running_time,
                                                        control: vs.get_control(),
                                                    });

                                                    let traffic_light = if vs
                                                        .has_traffic_light()
                                                    {
                                                        snapshot
                                                            .find_actor_snapshot(
                                                                vs.get_traffic_light_id(),
                                                            )
                                                            .map(|s| TrafficLight {
                                                                timestamp: running_time,
                                                                location_xyz: s.get_location(),
                                                                rotation_pyr: s.get_rotation(),
                                                                state: s
                                                                    .get_traffic_light_state(),
                                                            })
                                                    } else {
                                                        None
                                                    };

                                                    (chassis, traffic_light)
                                                });

                                                let collision_event =
                                                    base_sensors.cd.get_event();

                                                let lane_invasion_event =
                                                    base_sensors.lid.get_event();

                                                let camera_images = cameras
                                                    .iter_mut()
                                                    .filter_map(|(camera_id, camera)| {
                                                        camera
                                                            .get_image()
                                                            .map(|image| (camera_id, image))
                                                    })
                                                    .fold(
                                                        BTreeMap::new(),
                                                        |mut m, (camera_id, image)| {
                                                            m.insert(camera_id.clone(), image);
                                                            m
                                                        },
                                                    );

                                                (
                                                    id.to_string(),
                                                    if let Some((chassis, traffic_light)) = ct {
                                                        Sensors {
                                                            localization,
                                                            chassis,
                                                            collision_event,
                                                            lane_invasion_event,
                                                            traffic_light,
                                                            camera_images,
                                                        }
                                                    } else {
                                                        Sensors {
                                                            localization,
                                                            chassis: None,
                                                            collision_event,
                                                            lane_invasion_event,
                                                            traffic_light: None,
                                                            camera_images,
                                                        }
                                                    },
                                                )
                                            })
                                            .collect();

                                        let obstacles = snapshot
                                            .get_actor_ids()
                                            .iter()
                                            .filter_map(|id| {
                                                if let Ok(Some(actor)) =
                                                    world.get_actor(*id)
                                                {
                                                    let mut type_tag = 0u8;
                                                    for tag in actor.get_semantic_tags() {
                                                        if tag == 4u8 || tag == 10u8 {
                                                            type_tag = tag;
                                                            break;
                                                        }
                                                    }

                                                    if type_tag != 0u8 {
                                                        return Some((
                                                            *id,
                                                            type_tag,
                                                            actor.get_bounding_box(),
                                                        ));
                                                    }
                                                }

                                                None
                                            })
                                            .filter_map(|(id, type_tag, bounding_box)| {
                                                snapshot
                                                    .find_actor_snapshot(id)
                                                    .map(|o| (o, type_tag, bounding_box))
                                            })
                                            .map(|(o, type_tag, bounding_box)| {
                                                (
                                                    o.get_id().to_string(),
                                                    Obstacle {
                                                        type_tag,
                                                        is_static: false,
                                                        bounding_box,
                                                        location_xyz: o.get_location(),
                                                        rotation_pyr: o.get_rotation(),
                                                        linear_vel_xyz: o.get_velocity(),
                                                    },
                                                )
                                            })
                                            .fold(BTreeMap::new(), |mut m, (id, obstacle)| {
                                                m.insert(id, obstacle);
                                                m
                                            });

                                        Snapshot {
                                            running_time,
                                            vehicle_sensors,
                                            obstacles,
                                        }
                                    });

                                if res_tx.blocking_send(Response::GetSnapshot(r)).is_err() {
                                    break;
                                }
                            }
                        }
                    }
                } else {
                    error!("Logic error");
                }
            }

            Err(e) => {
                if res_tx
                    .blocking_send(Response::Ready(Err(Error::from(e))))
                    .is_err()
                {
                    error!("Logic error");
                }
            }
        }
    });

    // 等待线程就绪信号
    let proxy = if let Some(Response::Ready(r)) = res_rx.recv().await {
        r.map(|_| Inner { req_tx, res_rx })
    } else {
        Err(Error::new("Logic error".to_string()))
    };

    proxy
}

type ContextMt = Arc<Mutex<Context>>;

struct Context {
    vehicles: BTreeMap<u32, Vehicle>,
}

struct Vehicle {
    vehicle: carla::client::Vehicle,
    base_sensors: Option<BaseSensors>,
    cameras: BTreeMap<String, carla::client::Camera>,
}

struct BaseSensors {
    gnss: carla::client::GNSS,
    twist: Twist,
    cd: carla::client::CollisionDetector,
    lid: carla::client::LaneInvasionDetector,
}

/// 通过后向差分计算速度,角速度和加速度.
struct Twist {
    t: Option<u64>,
    trasform: Option<((f32, f32, f32), (f32, f32, f32))>,
    velocity: Option<((f32, f32, f32), (f32, f32, f32))>,
}

impl Twist {
    fn new() -> Self {
        Self {
            t: None,
            trasform: None,
            velocity: None,
        }
    }

    fn update(
        &mut self,
        t: u64,
        loc: (f32, f32, f32),
        rot: (f32, f32, f32),
    ) -> Option<((f32, f32, f32), (f32, f32, f32), (f32, f32, f32))> {
        if let Some(prev_t) = self.t.replace(t) {
            if let Some((prev_loc, prev_rot)) = self.trasform.replace((loc, rot)) {
                let dt = (t - prev_t) as f32 / 1e6;
                let v_x = (loc.0 - prev_loc.0) / dt;
                let v_y = (loc.1 - prev_loc.1) / dt;
                let v_z = (loc.2 - prev_loc.2) / dt;
                let v = (v_x, v_y, v_z);
                let av_x = (rot.2 - prev_rot.2) / dt;
                let av_y = (rot.0 - prev_rot.0) / dt;
                let av_z = (rot.1 - prev_rot.1) / dt;
                let av = (av_x, av_y, av_z);

                if let Some((prev_v, _)) = self.velocity.replace((v, av)) {
                    let a_x = (v.0 - prev_v.0) / dt;
                    let a_y = (v.1 - prev_v.1) / dt;
                    let a_z = (v.2 - prev_v.2) / dt;
                    let a = (a_x, a_y, a_z);

                    return Some((v, av, a));
                }
            }
        } else {
            self.trasform.replace((loc, rot));
        }

        None
    }
}

/// 代理.
pub struct Proxy {
    inner: Inner,
}

impl Proxy {
    /// 载入opendrive地图.
    ///
    /// content为xml格式字符串.
    pub async fn load_map(&mut self, content: String) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::LoadMap { content })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::LoadMap(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 创建车辆.
    ///
    /// 指定车辆型号,起始位姿(carla坐标系)创建车辆,返回车辆id.
    pub async fn spawn_vehicle(
        &mut self,
        vehicle_type: &str,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
    ) -> Result<String> {
        self.inner
            .req_tx
            .send(Request::SpawnVehicle {
                vehicle_type: vehicle_type.to_string(),
                location_xyz,
                rotation_pyr,
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::SpawnVehicle(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 根据角色名获取已存在车辆.
    ///
    /// 返回(车辆id,车辆型号).
    pub async fn get_vehicle_by_role(&mut self, role_name: &str) -> Result<(String, String)> {
        self.inner
            .req_tx
            .send(Request::GetVehicleByRole {
                role_name: role_name.to_string(),
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::GetVehicleByRole(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 启用车辆的carsim仿真.
    ///
    /// carsim_cfg_path为simfile文件路径.
    pub async fn enable_carsim(&mut self, vehicle_id: &str, carsim_cfg_path: &str) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::EnableCarsim {
                vehicle_id: vehicle_id.to_string(),
                carsim_cfg_path: carsim_cfg_path.to_string(),
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::EnableCarsim(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 启用车辆基本传感器.
    pub async fn enable_base_sensors(
        &mut self,
        vehicle_id: &str,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
        sensor_tick: f32,
    ) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::EnableBaseSensors {
                vehicle_id: vehicle_id.to_string(),
                location_xyz,
                rotation_pyr,
                sensor_tick,
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::EnableBaseSensors(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 启用摄像机.
    pub async fn enable_camera(
        &mut self,
        vehicle_id: &str,
        camera_id: &str,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
        fov: f32,
        image_size_x: u32,
        image_size_y: u32,
        iso: f32,
        gamma: f32,
        shutter_speed: f32,
        sensor_tick: f32,
    ) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::EnableCamera {
                vehicle_id: vehicle_id.to_string(),
                camera_id: camera_id.to_string(),
                location_xyz,
                rotation_pyr,
                fov,
                image_size_x,
                image_size_y,
                iso,
                gamma,
                shutter_speed,
                sensor_tick,
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::EnableCamera(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 设置车辆位置.
    pub async fn set_transform(
        &mut self,
        vehicle_id: &str,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
    ) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::SetTransform {
                vehicle_id: vehicle_id.to_string(),
                location_xyz,
                rotation_pyr,
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))
    }

    /// 删除车辆.
    pub async fn destroy_vehicle(&mut self, vehicle_id: &str, created: bool) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::DestroyVehicle {
                vehicle_id: vehicle_id.to_string(),
                created,
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::DestroyVehicle(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 删除所有车辆.
    pub async fn destroy_all_vehicles(&mut self) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::DestroyAllVehicles)
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::DestroyAllVehicles) = self.inner.res_rx.recv().await {
            Ok(())
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }

    /// 获取当前地图名称.
    pub async fn get_map_name(&mut self) -> Result<String> {
        self.inner
            .req_tx
            .send(Request::GetMapName)
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::GetMapName(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }
}

/// 传感器和车辆控制代理.
pub struct RuntimeProxy {
    inner: Inner,
}

impl RuntimeProxy {
    /// 设置车辆控制参数.
    pub async fn apply_vehicle_control(
        &mut self,
        vehicle_id: &str,
        control: &VehicleControl,
    ) -> Result<()> {
        self.inner
            .req_tx
            .send(Request::ApplyVehicleControl {
                vehicle_id: vehicle_id.to_string(),
                control: control.clone(),
            })
            .await
            .map_err(|_| Error::new("Logic error".to_string()))
    }

    /// 获取世界状态.
    pub async fn get_snapshot(&mut self) -> Result<Snapshot> {
        self.inner
            .req_tx
            .send(Request::GetSnapshot)
            .await
            .map_err(|_| Error::new("Logic error".to_string()))?;
        if let Some(Response::GetSnapshot(r)) = self.inner.res_rx.recv().await {
            r
        } else {
            Err(Error::new("Logic error".to_string()))
        }
    }
}

struct Inner {
    req_tx: mpsc::Sender<Request>,
    res_rx: mpsc::Receiver<Response>,
}

enum Request {
    LoadMap {
        content: String,
    },
    SpawnVehicle {
        vehicle_type: String,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
    },
    GetVehicleByRole {
        role_name: String,
    },
    EnableCarsim {
        vehicle_id: String,
        carsim_cfg_path: String,
    },
    EnableBaseSensors {
        vehicle_id: String,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
        sensor_tick: f32,
    },
    EnableCamera {
        vehicle_id: String,
        camera_id: String,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
        fov: f32,
        image_size_x: u32,
        image_size_y: u32,
        iso: f32,
        gamma: f32,
        shutter_speed: f32,
        sensor_tick: f32,
    },
    SetTransform {
        vehicle_id: String,
        location_xyz: (f32, f32, f32),
        rotation_pyr: (f32, f32, f32),
    },
    ApplyVehicleControl {
        vehicle_id: String,
        control: VehicleControl,
    },
    DestroyVehicle {
        vehicle_id: String,
        created: bool,
    },
    DestroyAllVehicles,
    GetMapName,
    GetSnapshot,
}

pub type VehicleControl = carla::client::Control;

enum Response {
    Ready(Result<()>),
    LoadMap(Result<()>),
    SpawnVehicle(Result<String>),
    GetVehicleByRole(Result<(String, String)>),
    EnableCarsim(Result<()>),
    EnableBaseSensors(Result<()>),
    EnableCamera(Result<()>),
    SetTransform(Result<()>),
    DestroyVehicle(Result<()>),
    DestroyAllVehicles,
    GetMapName(Result<String>),
    GetSnapshot(Result<Snapshot>),
}

/// 世界状态.
pub struct Snapshot {
    /// 从当前地图加载开始的运行时间.
    pub running_time: u64,
    /// 车辆传感器数据
    pub vehicle_sensors: BTreeMap<String, Sensors>,
    /// 障碍物数据
    pub obstacles: BTreeMap<String, Obstacle>,
}

pub struct Sensors {
    pub localization: Option<Localization>,
    pub chassis: Option<Chassis>,
    pub collision_event: Option<CollisionEvent>,
    pub lane_invasion_event: Option<LaneInvasionEvent>,
    pub traffic_light: Option<TrafficLight>,
    pub camera_images: BTreeMap<String, Image>,
}

pub type CollisionEvent = carla::client::CollisionEvent;

pub type LaneInvasionEvent = carla::client::LaneInvasionEvent;

pub type Image = carla::client::Image;

pub struct Localization {
    pub base: carla::client::Localization,
    pub velocity: (f32, f32, f32),
    pub angular_velocity: (f32, f32, f32),
    pub acceleration: (f32, f32, f32),
}

pub struct Chassis {
    pub timestamp: u64,
    pub control: Control,
}

pub type Control = carla::client::Control;

pub struct TrafficLight {
    pub timestamp: u64,
    pub location_xyz: (f32, f32, f32),
    pub rotation_pyr: (f32, f32, f32),
    pub state: i32,
}

pub struct Obstacle {
    pub type_tag: u8,
    pub is_static: bool,
    pub bounding_box: carla::client::BoundingBox,
    pub location_xyz: (f32, f32, f32),
    pub rotation_pyr: (f32, f32, f32),
    pub linear_vel_xyz: (f32, f32, f32),
}
